//
// Created by cch on 2022/4/26.
//

#ifndef FRONT_MATCH_TRANGULATE_H
#define FRONT_MATCH_TRANGULATE_H
#include "FeatureMatch.h"

struct SFMFeature {
  bool state;
  int id;
  std::vector<std::pair<int, Eigen::Vector2d>> obs;
  double position[3];
  double depth;
};

class Trangulate {

public:
  Trangulate() {

    m_LKMatch = new LKMatch();
    GetLkMatch()->KeyPointMatch();
    cvToEigen(Point1, GetLkMatch()->GetLastPoints());
    cvToEigen(Point2, GetLkMatch()->GetCurPoints());
    assert(Point1.size() != 0 && Point2.size() != 0);
  }
  void EstimateLandMarkByOpencv() {
    //位姿有了还缺特征点
    Pose1 = Eigen::Matrix<double, 3, 4>::Identity();
    Pose2.block<3, 3>(0, 0) = GetRotation();
    Pose2.block<3, 1>(0, 3) = GetTranslation();

    cv::Mat T1, T2, pos_4d;
    eigen2cv(Pose1, T1);
    eigen2cv(Pose2, T2);
    cv::triangulatePoints(T1, T2, GetLkMatch()->GetLastPoints(),
                          GetLkMatch()->GetCurPoints(), pos_4d);

    for (int i = 0; i < pos_4d.cols; ++i) {
      cv::Mat x = pos_4d.col(i);
      x /= x.at<float>(3, 0);
      cv::Point3d point3D(x.at<float>(0, 0), x.at<float>(1, 0),
                          x.at<float>(2, 0));
      Point3d_CV.push_back(point3D);
    }
    // std::cout << Point3d_CV << std::endl;
  }
  void EstimateLandMarkBySelf() {
    Pose1 = Eigen::Matrix<double, 3, 4>::Identity();
    Pose2.block<3, 3>(0, 0) = GetRotation();
    Pose2.block<3, 1>(0, 3) = GetTranslation();

    //对所有特征点进行恢复
    for (int i = 0; i < Point1.size(); i++)
      triangulatePoint(Pose1, Pose2, Point1[i], Point2[i]);
  }
  inline LKMatch *GetLkMatch() { return m_LKMatch; }

  inline Matrix3d GetRotation() { return GetLkMatch()->GetRotation(); }

  inline Vector3d GetTranslation() { return GetLkMatch()->GetTranslation(); }

  void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0,
                        Eigen::Matrix<double, 3, 4> &Pose1, Vector2d &point0,
                        Vector2d &point1) {
    // 通过奇异值分解求解一个Ax = 0得到
    Matrix4d design_matrix = Matrix4d::Zero();
    design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
    design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
    design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
    design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
    Eigen::Vector4d triangulated_point =
        design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
    // 齐次向量归一化
    Eigen::Vector3d point3d;
    point3d(0) = triangulated_point(0) / triangulated_point(3);
    point3d(1) = triangulated_point(0) / triangulated_point(3);
    point3d(2) = triangulated_point(2) / triangulated_point(3);

    Point3d.push_back(point3d);

    //    point_3d.x() = triangulated_point(0) / triangulated_point(3);
    //    point_3d(1) = triangulated_point(1) / triangulated_point(3);
    //    point_3d(2) = triangulated_point(2) / triangulated_point(3);
  }

  void cvToEigen(std::vector<Vector2d> &Points,
                 const std::vector<cv::Point2f> &Pts) {
    Points.resize(Pts.size());
    for (size_t i = 0; i < (size_t)Pts.size(); ++i) {
      Points.at(i).x() = static_cast<double>(Pts[i].x);
      Points.at(i).y() = static_cast<double>(Pts[i].y);
    }
  }

  ~Trangulate() { delete m_LKMatch; }

private:
  //当前帧位姿和后一帧位姿
  Eigen::Matrix<double, 3, 4> Pose1, Pose2;

  std::vector<Eigen::Vector2d> Point1, Point2;
  std::vector<Eigen::Vector3d> Point3d;
  std::vector<cv::Point3d> Point3d_CV;

  LKMatch *m_LKMatch;
};

#endif // FRONT_MATCH_TRANGULATE_H
